/*
 * CCVisu is a tool for visual graph clustering
 * and general force-directed graph layout.
 * This file is part of CCVisu.
 *
 * Copyright (C) 2005-2011  Dirk Beyer
 *
 * CCVisu is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * CCVisu is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with CCVisu; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Please find the GNU Lesser General Public License in file
 * license_lgpl.txt or http://www.gnu.org/licenses/lgpl.txt
 *
 * Andreas Noack (an@informatik.tu-cottbus.de)
 * University of Technology at Cottbus, Germany
 * Dirk Beyer    (firstname.lastname@uni-passau.de)
 * University of Passau, Bavaria, Germany
 */
package org.sosy_lab.ccvisu.layout;

import java.util.EventObject;
import java.util.List;

import org.sosy_lab.ccvisu.Options;
import org.sosy_lab.ccvisu.Options.OptionsEnum;
import org.sosy_lab.ccvisu.Options.Verbosity;
import org.sosy_lab.ccvisu.graph.GraphData;
import org.sosy_lab.ccvisu.graph.GraphEdge;
import org.sosy_lab.ccvisu.graph.GraphVertex;
import org.sosy_lab.ccvisu.graph.Position;

/**
 * Minimizer for the (weighted) (edge-repulsion) LinLog energy model,
 * based on the Barnes-Hut algorithm.
 *
 * Changed:
 * Extended to edge-repulsion, according to the IWPC 2005 paper.
 * Data structures changed to achieve O(n log n) space complexity.
 * Energy model extended to a weighted version.
 *
 * 2006-02-08:
 * Energy model changed to flexible repulsion exponent.
 */
public class MinimizerBarnesHut extends Minimizer {

  /**
   * The graph that this layout is computed for.
   *    node. fixedPos: The minimizer does not change the position
   *    of a node with fixedPos == true.
   *    pos: Position matrix (3 dimensions for every node).
   *    Is not copied and serves as input and output
   *    of <code>minimizeEnergy</code>.
   *    If the input is two-dimensional (i.e. pos[i][2] == 0
   *    for all i), the output is also two-dimensional.
   *    Random initial positions are appropriate.
   *    Preconditions: dimension at least [nodeNr][3];
   *    no two different nodes have the same position
   *    The input positions should be scaled such that
   *    the average Euclidean distance between connected nodes
   *    is roughly 1.
   s*/
  private final GraphData      graph;

  /** The following two must be symmetric. */

  /**
   * Node indexes of the similarity list for each node.
   *    Is not copied and not modified by minimizeEnergy.
   *    (attrIndexes[i][k] == j) represents the k-th edge
   *    of node i, namely (i,j).
   *    Omit edges with weight 0.0 (i.e. non-edges).
   *    Preconditions:
   *    (attrIndexes[i][k] != i) for all i,k (irreflexive);
   *    (attrIndexes[i][k1] == j) iff (attrIndexes[j][k2] == i)
   *     for all i, j, exists k1, k2 (symmetric).
   */
  private final int            attrIndexes[][];

  /** Similarity values of the similarity lists for each node.
   *    Is not copied and not modified by minimizeEnergy.
   *    For each (attrIndexes[i][k] == j), (attrValues[i][k] == w)
   *    represents the weight w of edge (i,j).
   *    For unweighted graphs use only 1.0f as edge weight.
   *    Preconditions:
   *    exists k1: (attrIndexes[i][k1] == j) and (attrValues[i][k1] == w) iff
   *    exists k2: (attrIndexes[j][k2] == i) and (attrValues[j][k2] == w)
   *    (symmetric).
   */
  private final float          attrValues[][];

  /** Repulsion vector (node weights).
   *    Is not copied and not modified by minimizeEnergy.
   *    For for node repulsion use 1.0f for all nodes.
   *    Preconditions: dimension at least [nodeNr];
   *    repu[i] >= 1 for all i.
   */
  private final float          repu[];

  /** Exponent of the Euclidean distance in the attraction term of the energy model.
   *    1.0f for the LinLog models, 3.0f for the energy
   *    version of the Fruchterman-Reingold model.
   *    If 0.0f, the log of the distance is taken instead of a constant fun.
   *    (Value 0.0f not yet tested.)
   */
  private float                attrExponent      = 1.0f;

  /** Exponent of the Euclidean distance in the repulsion term of the energy model.
   *     0.0f for the LinLog models, 0.0f for the energy
   *     version of the Fruchterman-Reingold model.
   *     If 0.0f, the log of the distance is taken instead of a constant fun.
   */
  private float                repuExponent      = 0.0f;

  /** Position of the barycenter of the nodes. */
  private Position             baryCenter        = new Position();

  /** Factor for the gravitation energy (attraction to the barycenter),
   *    0.0f for no gravitation. */
  private float                gravitationFactor = 0.0f;

  /** Factor for repulsion energy that normalizes average distance
   * between pairs of nodes with maximum similarity to (roughly) 1. */
  private float                repuFactor        = 1.0f;

  /** Factors for the repulsion force for pulsing. */
  private static final float[] repuStrategy      = { 1.0f, 1.1f, 1.2f, 1.3f,
                                                     1.4f, 1.5f, 1.4f, 1.3f,
                                                     1.2f, 1.1f, 1.0f, 0.95f,
                                                     0.9f, 0.85f, 0.8f, 0.75f,
                                                     0.8f, 0.85f, 0.9f, 0.95f };

  /** OctTree for repulsion computation. */
  private OctTree              octTree           = null;

  /**
   * Sets the number of nodes, the similarity matrices (edge weights), and the
   * position matrix.
   */
  public MinimizerBarnesHut(Options options) {
    super(options);

    graph = options.graph;
    attrExponent = options.attrExponent;
    repuExponent = options.repuExponent;
    gravitationFactor = options.gravitation;

    // Create graph layout data structure, allocate memory.
    // Positions are already initialized (given by graph).

    int vertexCount = graph.getVertices().size();

    // Initialize repulsions.
    repu = new float[vertexCount];
    for (int i = 0; i < vertexCount; ++i) {
      // Set repulsion according to the energy model.
      if (options.vertRepu) {
        repu[i] = 1.0f;
      } else {
        GraphVertex graphVertex = graph.getVertices().get(i);
        repu[i] = graphVertex.getDegree();
      }
    }

    // Initialize Attractions in steps 1-4

    // 1. Allocate the columns.
    // Vertex indexes of the similarity lists.
    attrIndexes = new int[vertexCount][];
    // Similarity values of the similarity lists.
    attrValues = new float[vertexCount][];

    // 2. Compute length of rows, i.e., the number of in- and outgoing
    // non-reflexive edges for every GraphVertex.
    int[] attrCounter = new int[vertexCount];
    for (GraphEdge edge : graph.getEdges()) {
      if (edge.getSource() == edge.getTarget()) {
        // We must not add reflexive edges.
      } else {
        ++attrCounter[edge.getSource().getId()];
        ++attrCounter[edge.getTarget().getId()];
      }
    }

    // 3. Allocate the rows (see step 2).
    for (int i = 0; i < vertexCount; i++) {
      attrIndexes[i] = new int[attrCounter[i]];
      attrValues[i] = new float[attrCounter[i]];
    }

    // 4. Transfer the edges to the similarity lists, i.e., fill the rows.
    int[] edgeCounter = new int[vertexCount];
    for (GraphEdge edge : graph.getEdges()) {
      if (edge.getSource() == edge.getTarget()) {
        // We must not add reflexive edges.
      } else {
        int sourceId = edge.getSource().getId();
        int targetId = edge.getTarget().getId();

        // Similarity list must be symmetric.
        attrIndexes[sourceId][edgeCounter[sourceId]] = targetId;
        attrIndexes[targetId][edgeCounter[targetId]] = sourceId;

        // Set similarities according to the energy model.
        if (options.noWeight) {
          attrValues[sourceId][edgeCounter[sourceId]] = 1.0f;
          attrValues[targetId][edgeCounter[targetId]] = 1.0f;
        } else {
          attrValues[sourceId][edgeCounter[sourceId]] = edge.getWeight();
          attrValues[targetId][edgeCounter[targetId]] = edge.getWeight();
        }

        ++edgeCounter[sourceId];
        ++edgeCounter[targetId];
      }
    }
  }

  /**
   * Iteratively minimizes energy using the Barnes-Hut algorithm.
   * Starts from the positions in <code>pos</code>,
   * and stores the computed positions in <code>pos</code>.
   * @throws InterruptedException
   */
  @Override
  public void minimizeEnergy() {
    int nodeCount = graph.getVertices().size();
    if (nodeCount <= 1) {
      return;
    }

    int numberOfIterations = options.getOption(OptionsEnum.iter).getInt();


    if (options.verbosity.isAtLeast(Verbosity.WARNING)) {
      System.err.println();
      System.err.println("Note: Minimizer will run " + numberOfIterations + " iterations,");
      System.err.println("      increase (decrease) this number with option -iter to");
      System.err.println("      increase quality of layout (decrease runtime).");
    }

    if ((numberOfIterations == 0)
        && (options.getOption(OptionsEnum.disableDISPGuiControls).getBool())) {
      // We don't need to minimize the energy and the gui controls are disabled,
      // so just return.
      return;
    }

    setProgress(0, nodeCount, "Analyzing distances.");
    // Print stats.
    analyzeDistances();

    final float finalRepuFactor = computeRepuFactor();
    repuFactor = finalRepuFactor;

    // compute initial energy
    setProgress(0, nodeCount, "Computing initial energy.");
    buildOctTree();
    float energySum = 0.0f;

    for (int i = 0; i < nodeCount; i++) {
      energySum += getEnergy(i);
    }

    if (options.verbosity.isAtLeast(Verbosity.WARNING)) {
      System.err.println();
      System.err.println("initial energy: " + energySum + "; repulsion: " + repuFactor);
    }

    // Draw initial layout before minimizing
    notifyAboutChangedLayout(false);

    System.out.println("Starting minimizer.");
    performMinimization(nodeCount, numberOfIterations, finalRepuFactor, energySum);

    System.out.println("Minimizer finished.");
    setProgress(1, 1, "Minimizer finished.");

    // Print stats.
    analyzeDistances();
  }

  /**
   * This method performs the minimization prepared by method minimizeEnergy.
   */
  private void performMinimization(int nodeCount, int numberOfIterations,
      final float finalRepuFactor, float energySum) {

    int iterationsLeft = numberOfIterations;

    // minimize energy
    float prevEnergy = energySum;
    int step = 0;

    //float energyMinimum = Float.MAX_VALUE;
    //int energyMinimumOnIterationNumber = -1;

    while (true) {
      // Maybe the thread is interrupted by the caller.
      if (Thread.interrupted()) {
        return;
      }

      // Notify about the progress of the thread.
      setProgress(step, numberOfIterations, "Minimizing energy.");

      // Determine mode of operation.
      if (iterationsLeft > 0) {
        // Normal iteration of the minimizer.
        step++;
        iterationsLeft--;
      } else {
        break;
      }

      computeBaryCenter();
      buildOctTree();

      if (options.getOption(OptionsEnum.energyDiffThreshold).getFloat() == 0.0f) {
        // except in the last 20 iterations, vary the repulsion factor
        // according to repuStrategy
        if ((iterationsLeft + 1) > 20) {
          int index = (iterationsLeft + 1) % MinimizerBarnesHut.repuStrategy.length;

          repuFactor = finalRepuFactor
            * (float) Math.pow(MinimizerBarnesHut.repuStrategy[index], attrExponent - repuExponent);
        } else {
          repuFactor = finalRepuFactor;
        }
      } else {
        repuFactor = finalRepuFactor;
      }

      // for all non-fixed nodes: minimize energy, i.e., move each node
      energySum = 0.0f;
      for (int i = 0; i < nodeCount; i++) {
        GraphVertex vertex = graph.getVertices().get(i);
        if (!vertex.hasFixedPos()) {

          // compute direction of the move of the node
          Position bestDir = new Position();
          getDirection(i, bestDir);

          // line search: compute length of the move
          Position oldPos = new Position(vertex.getPosition());
          float oldEnergy = getEnergy(i);
          float bestEnergy = oldEnergy;
          int bestMultiple = 0;
          bestDir.div(32);

          for (int multiple = 32; multiple >= 1 && (bestMultiple == 0 || bestMultiple / 2 == multiple); multiple /= 2) {
            // vertex.position = oldPos + bestDir * multiple;
            vertex.setPosition(Position.add(oldPos, Position.mult(bestDir, multiple)));
            float energy = getEnergy(i);
            if (energy < bestEnergy) {
              bestEnergy = energy;
              bestMultiple = multiple;
            }
          }

          for (int multiple = 64; multiple <= 128 && bestMultiple == multiple / 2; multiple *= 2) {
            // vertex.position = oldPos + bestDir * multiple;
            vertex.setPosition(Position.add(oldPos, Position.mult(bestDir, multiple)));
            float energy = getEnergy(i);
            if (energy < bestEnergy) {
              bestEnergy = energy;
              bestMultiple = multiple;
            }
          }

          // vertex.pos = oldPos + bestDir * bestMultiple;
          vertex.setPosition(Position.add(oldPos, Position.mult(bestDir, bestMultiple)));
          if (bestMultiple > 0) {
            octTree.moveNode(oldPos, vertex.getPosition(), repu[i]); //1.0f);
          }
          energySum += bestEnergy;
        }
      } // for

      if (options.verbosity.isAtLeast(Verbosity.WARNING)) {
        System.out.println("iteration\t" + step + "\tenergy\t" + energySum
            + "\trepulsion\t" + repuFactor);
        setProgress(step, numberOfIterations, "Minimizing energy.");
      }

      if (options.getOption(OptionsEnum.energyDiffThreshold).getFloat() != 0.0f) {
        float energyDiff = Math.abs(prevEnergy - energySum) / nodeCount;

        if (energyDiff < options.getOption(OptionsEnum.energyDiffThreshold).getFloat()) {
          iterationsLeft = 0;
        }
      }

// Note: I also commented out the unused declarations of energyMinimum and
// energyMinimumOnIterationNumber.  @see: begin of method
//      if (energyMinimum > energySum) {
//        energyMinimumOnIterationNumber = step;
//        energyMinimum = energySum;
//      } else {
//        int minAtEarlierIterationThan = (int)(step * 0.9f);
//        if (step > 100 && (energyMinimumOnIterationNumber < minAtEarlierIterationThan)) {
//          iterationsLeft = 0;
//          System.out.println(String.format("Minimum %e on step %d becuase < %d", energyMinimum, energyMinimumOnIterationNumber, minAtEarlierIterationThan));
//        }
//      }

      prevEnergy = energySum;

      notifyAboutChangedLayout(iterationsLeft == 0);
    }
  }

  private void notifyAboutChangedLayout(boolean forceNotify) {
    if ((options.getOption(OptionsEnum.anim).getBool()) || forceNotify) {
      graph.notifyAboutLayoutChange(new EventObject(this));
    }
  }


  /**
   * Returns the repulsion energy between the node with the specified index
   * and the nodes in the octtree.
   *
   * @param index Index of the repulsing node.
   * @param tree  Octtree containing repulsing nodes.
   * @return Repulsion energy between the node with the specified index
   *         and the nodes in the octtree.
   */
  private float getRepulsionEnergy(final Position vertexIndexPos, final int index, OctTree tree) {
    if (tree == null || tree.index == index || index >= repu.length) {
      return 0.0f;
    }

    float dist = vertexIndexPos.distanceTo(tree.position);
    if (tree.index < 0 && dist < 2.0f * tree.width()) {
      float energy = 0.0f;
      for (OctTree lElement : tree.children) {
        energy += getRepulsionEnergy(vertexIndexPos, index, lElement);
      }
      return energy;
    }

    if (repuExponent == 0.0f) {
      return -repuFactor * tree.weight * (float) Math.log(dist) * repu[index];
    } else {
      return -repuFactor * tree.weight * (float) Math.pow(dist, repuExponent) / repuExponent * repu[index];
    }
  }

  /**
   * Returns the energy of the specified node.
   * @param   index   Index of a node.
   * @return  Energy of the node.
   */
  private float getEnergy(final int index) {
    List<GraphVertex> vertices = graph.getVertices();
    GraphVertex vertex = vertices.get(index);
    Position vertexPosition = vertex.getPosition();

    // repulsion energy
    float energy = getRepulsionEnergy(vertexPosition, index, octTree);

    // attraction energy
    for (int i = 0; i < attrIndexes[index].length; i++) {
      if (attrIndexes[index][i] != index) {
        float dist = vertices.get(attrIndexes[index][i]).distanceTo(vertexPosition);
        if (attrExponent == 0.0f) {
          energy += attrValues[index][i] * (float) Math.log(dist);
        } else {
          energy += attrValues[index][i] * (float) Math.pow(dist, attrExponent) / attrExponent;
        }
      }
    }

    // gravitation energy
    float dist = vertexPosition.distanceTo(baryCenter);
    if (attrExponent == 0.0f) {
      energy += gravitationFactor * repuFactor * repu[index] * (float) Math.log(dist);
    } else {
      energy += gravitationFactor * repuFactor * repu[index] * (float) Math.pow(dist, attrExponent) / attrExponent;
    }
    return energy;
  }

  /**
   * Computes the direction of the repulsion force from the tree
   *     on the specified node.
   * @param  index   Index of the repulsed node.
   * @param  tree    Repulsing octtree.
   * @param  dir     Direction of the repulsion force acting on the node
   *                 is added to this variable (output parameter).
   * @return Approximate second derivation of the repulsion energy.
   */
  private float addRepulsionDir(final int index, OctTree tree, Position dir) {

    if (tree == null || tree.index == index) {
      return 0.0f;
    }

    GraphVertex vertex = graph.getVertices().get(index);
    float dist = vertex.distanceTo(tree.position);

    if (tree.index < 0 && dist < tree.width()) {
      float dir2 = 0.0f;
      for (OctTree lElement : tree.children) {
        dir2 += addRepulsionDir(index, lElement, dir);
      }
      return dir2;
    }

    if (dist != 0.0) {
      float tmp = repuFactor * tree.weight * repu[index] * (float) Math.pow(dist, repuExponent - 2);
      // dir -= (tree.position - lVertexIndex) * tmp;
      dir.subtract(Position.mult(Position.subtract(tree.position, vertex.getPosition()), tmp));
      return tmp * Math.abs(repuExponent - 1);
    }

    return 0.0f;
  }

  /**
   * Computes the direction of the total force acting on the specified node.
   * @param  index   Index of a node.
   * @param  dir     Direction of the total force acting on the node
   *                 (output parameter).
   */
  private void getDirection(final int index, Position dir) {
    Position vertexPosition = graph.getVertices().get(index).getPosition();
    dir.x = 0.0f;
    dir.y = 0.0f;
    dir.z = 0.0f;

    // compute repulsion force vector
    float dir2 = addRepulsionDir(index, octTree, dir);

    // compute attraction force vector
    for (int i = 0; i < attrIndexes[index].length; i++) {
      if (attrIndexes[index][i] != index) {

        GraphVertex vertex = graph.getVertices().get(attrIndexes[index][i]);
        float dist = vertex.distanceTo(vertexPosition);
        float tmp = attrValues[index][i] * (float) Math.pow(dist, attrExponent - 2);

        // dir += (graph.vertices.get(attrIndexes[index][i]).pos - lVertexIndexPos)
        //         * tmp;
        dir.add(Position.mult(Position.subtract(vertex.getPosition(), vertexPosition),
            tmp));

        dir2 += tmp * Math.abs(attrExponent - 1);
      }
    }

    // compute gravitation force vector
    float dist = vertexPosition.distanceTo(baryCenter);
    dir2 +=
      gravitationFactor * repuFactor * repu[index] * (float) Math.pow(dist, attrExponent - 2)
      * Math.abs(attrExponent - 1);

    // dir +=  gravitationFactor * repuFactor * repu[index]
    //             * (float) Math.pow(dist, attrExponent - 2)
    //             * (baryCenter - lVertexIndexPos);
    dir.add(Position.mult(Position.subtract(baryCenter, vertexPosition),
        gravitationFactor * repuFactor * repu[index]
                                                                                                                * (float) Math.pow(dist, attrExponent - 2)));

    // normalize force vector with second derivation of energy
    dir.div(dir2);

    // ensure that the length of dir is at most 1/8
    // of the maximum Euclidean distance between nodes
    float length = (float) Math.sqrt(dir.x * dir.x + dir.y * dir.y + dir.z * dir.z);
    if (length > octTree.width() / 8) {
      length /= octTree.width() / 8;
      dir.div(length);
    }
  }

  /**
   * Builds the octtree.
   */
  private void buildOctTree() {
    // compute minima and maxima of positions in each dimension
    Position minPos = Position.min(graph.getVertices(), false);
    Position maxPos = Position.max(graph.getVertices(), false);

    // add nodes to the octtree
    octTree = new OctTree(0, graph.getVertices().get(0).getPosition(), repu[0], minPos, maxPos);
    int nodeCount = this.graph.getVertices().size();

    for (int i = 1; i < nodeCount; i++) {
      octTree.addNode(i, graph.getVertices().get(i).getPosition(), repu[i]); // 1.0f);
    }
  }

  /**
   * Computes the factor for repulsion forces <code>repuFactor</code>
   * such that in the energy minimum the average Euclidean distance
   * between pairs of nodes with similarity 1.0 is approximately 1.
   */
  private float computeRepuFactor() {
    float attrSum = 0.0f;
    int nodeCount = this.graph.getVertices().size();

    for (int i = 1; i < nodeCount; i++) {
      for (int j = 0; j < attrValues[i].length; j++) {
        attrSum += attrValues[i][j];
      }
    }

    float repuSum = 0.0f;
    for (int i = 0; i < nodeCount; i++) {
      repuSum += repu[i];
    }

    if (repuSum > 0 && attrSum > 0) {
      return attrSum / (repuSum * repuSum)
          * (float) Math.pow(repuSum, 0.5f * (attrExponent - repuExponent));
    }

    return 1.0f;
  }

  /**
   * Computes the position of the barycenter <code>baryCenter</code>
   * of all nodes.
   */
  private void computeBaryCenter() {
    // Reset.
    int vertexCount = 0;
    baryCenter = new Position();
    for (GraphVertex vertex : graph.getVertices()) {
      baryCenter.add(vertex.getPosition());
      vertexCount++;
    }
    baryCenter.div(vertexCount);
  }

  /**
   * Computes and outputs some statistics.
   */
  private void analyzeDistances() {
    float edgeLengthSum = 0.0f;
    float edgeLengthLogSum = 0.0f;
    float attrSum = 0.0f;

    int nodeCount = graph.getVertices().size();
    for (int i = 0; i < nodeCount; i++) {
      GraphVertex vertex = graph.getVertices().get(i);

      for (int j = 0; j < attrValues[i].length; j++) {
        float dist = vertex.distanceTo(graph.getVertices().get(attrIndexes[i][j]));
        float distLog = (float) Math.log(dist);
        edgeLengthSum += attrValues[i][j] * dist;
        edgeLengthLogSum += attrValues[i][j] * distLog;
        attrSum += attrValues[i][j];
      }
    }

    edgeLengthSum /= 2;
    edgeLengthLogSum /= 2;
    attrSum /= 2;

    if (options.verbosity.isAtLeast(Verbosity.WARNING)) {
      System.err.println();
      System.err.println("Number of Nodes: " + nodeCount);
      System.err.println("Overall Attraction: " + attrSum);
      System.err.println("Arithmetic mean of edge lengths: "
          + edgeLengthSum / attrSum);
      System.err.println("Geometric mean of edge lengths: "
          + (float) Math.exp(edgeLengthLogSum / attrSum));
    }
  }

  /**
   * Octtree for graph nodes with positions in 3D space.
   * Contains all graph nodes that are located in a given cuboid in 3D space.
   */
  public static class OctTree {
    /** For leafs, the unique index of the graph node; for non-leafs -1. */
    private int             index;
    /** Children of this tree node. */
    private final OctTree[] children = new OctTree[8];
    /** Barycenter of the contained graph nodes. */
    private Position        position;
    /** Total weight of the contained graph nodes. */
    private float           weight;
    /** Minimum coordinates of the cuboid in each of the 3 dimensions. */
    private final Position  minPos;
    /** Maximum coordinates of the cuboid in each of the 3 dimensions. */
    private final Position  maxPos;

    /**
     * Creates an octtree containing one graph node.
     *
     * @param index    Unique index of the graph node.
     * @param position Position of the graph node.
     * @param weight   Weight of the graph node.
     * @param minPos   Minimum coordinates of the cuboid.
     * @param maxPos   Maximum coordinates of the cuboid.
     */
    public OctTree(int index, Position position, float weight, Position minPos,
        Position maxPos) {

      this.index = index;
      this.position = new Position(position);
      this.weight = weight;
      this.minPos = minPos;
      this.maxPos = maxPos;
    }

    /**
     * Adds a graph node to the octtree.
     *
     * @param nodeIndex  Unique index of the graph node.
     * @param nodePos    Position of the graph node.
     * @param nodeWeight Weight of the graph node.
     */
    private void addNode(int nodeIndex, Position nodePos, float nodeWeight) {
      if (nodeWeight == 0.0f) {
        return;
      }

      if (index >= 0) {
        addNode2(index, position, weight);
        index = -1;
      }

      // position = (position * weight + nodePos * nodeWeight)
      //                / (weight + nodeWeight)
      position = Position.div(Position.add(Position.mult(position, weight),
          Position.mult(nodePos, nodeWeight)), weight + nodeWeight);

      weight += nodeWeight;

      addNode2(nodeIndex, nodePos, nodeWeight);
    }

    /**
     * Adds a graph node to the octtree,
     * without changing the position and weight of the root.
     *
     * @param nodeIndex  Unique index of the graph node.
     * @param nodePos    Position of the graph node.
     * @param nodeWeight Weight of the graph node.
     */
    private void addNode2(int nodeIndex, Position nodePos, float nodeWeight) {
      int childIndex = 0;

      if (nodePos.x > (minPos.x + maxPos.x) / 2) {
        childIndex += 1 << 0;
      }

      if (nodePos.y > (minPos.y + maxPos.y) / 2) {
        childIndex += 1 << 1;
      }

      if (nodePos.z > (minPos.z + maxPos.z) / 2) {
        childIndex += 1 << 2;
      }

      if (children[childIndex] == null) {
        Position newMinPos = new Position();
        Position newMaxPos = new Position();

        if ((childIndex & 1 << 0) == 0) {
          newMinPos.x = minPos.x;
          newMaxPos.x = (minPos.x + maxPos.x) / 2;
        } else {
          newMinPos.x = (minPos.x + maxPos.x) / 2;
          newMaxPos.x = maxPos.x;
        }

        if ((childIndex & 1 << 1) == 0) {
          newMinPos.y = minPos.y;
          newMaxPos.y = (minPos.y + maxPos.y) / 2;
        } else {
          newMinPos.y = (minPos.y + maxPos.y) / 2;
          newMaxPos.y = maxPos.y;
        }

        if ((childIndex & 1 << 2) == 0) {
          newMinPos.z = minPos.z;
          newMaxPos.z = (minPos.z + maxPos.z) / 2;
        } else {
          newMinPos.z = (minPos.z + maxPos.z) / 2;
          newMaxPos.z = maxPos.z;
        }

        children[childIndex] = new OctTree(nodeIndex, nodePos, nodeWeight,
            newMinPos, newMaxPos);

      } else {
        children[childIndex].addNode(nodeIndex, nodePos, nodeWeight);
      }
    }

    /**
     * Updates the positions of the octtree nodes
     * when the position of a graph node has changed.
     *
     * @param oldPos     Previous position of the graph node.
     * @param newPos     New position of the graph node.
     * @param nodeWeight Weight of the graph node.
     */
    private void moveNode(Position oldPos, Position newPos, float nodeWeight) {

      //position += (newPos - oldPos) * (nodeWeight / weight);
      position.add(Position.mult(Position.subtract(newPos, oldPos),
          nodeWeight / weight));

      int childIndex = 0;

      if (oldPos.x > (minPos.x + maxPos.x) / 2) {
        childIndex += 1 << 0;
      }

      if (oldPos.y > (minPos.y + maxPos.y) / 2) {
        childIndex += 1 << 1;
      }

      if (oldPos.z > (minPos.z + maxPos.z) / 2) {
        childIndex += 1 << 2;
      }

      if (children[childIndex] != null) {
        children[childIndex].moveNode(oldPos, newPos, nodeWeight);
      }
    }

    /**
     * Returns the maximum extension of the octtree.
     *
     * @return Maximum over all dimensions of the extension of the octtree.
     */
    private float width() {
      return Position.width(maxPos, minPos);
    }
  }
}
